# include "linalg.hpp"

# include <iostream>
# include <string>

// Lapack functions
extern "C" {

  // SVD decomposition
  void dgesdd_(char*,   int*,    int*, double*, int*, 
               double*, double*, int*, double*, int*, 
               double*, int*,    int*, int*);

  // LU decomposition of square matrix
  void (dgetrf_)(int*, int*, double*, int*, int*, int*);

  // solve system with triangular matrix
  void (dgetrs_)(char*, int*, int*, double*, int*, int*, double*, int*, int*);

}



/*! @brief Resize matrix
 *
 * @param[in] nr number of rows 
 * @param[in] nc number of columns
 */
void linalg::Matrix::resize(size_t const nr, size_t const nc) {
  //
  // invalidate all the containers and iterators
  //
  data_.resize(nr*nc);
  rows_ = nr;
  cols_ = nc;
}


/*! @brief Compute LUD of the matrix
 */
void linalg::Matrix::LUD() {
  int info = 0;
  permutation_.resize(cols_);

  int cols = cols_, rows = rows_;
  dgetrf_(&cols, &rows, &data_[0], &cols, &permutation_[0], &info);
}



/*! @brief Compute SVD of the matrix
 *
 * @param[in] M matrix 
 * @param[out] u
 * @param[out] s singular values of matrix
 * @param[out] vt transpose version 
 */
void linalg::svd(Matrix const& M, Matrix& u, Matrix& s, Matrix& vt) {
  int rows = static_cast<int>(M.rows());
  int cols = static_cast<int>(M.cols());
  int lda = std::max(rows, cols);

  int valuesNumber = std::min(rows, cols);

  // Reserve space for the results
  u.resize(M.rows(), M.rows());
  s.resize(valuesNumber, 1);
  vt.resize(M.cols(), M.cols());

  // Workspace and status variables:
  double workSize = 0;
  double *testWork = &workSize;
  int lwork = -1;
  std::vector<int> iwork(8*valuesNumber);
  int info = 0;

  char workType = 'A';
  // Call dgesdd_ with lwork = -1 to query optimal workspace size:
  dgesdd_(&workType, &rows, &cols, 
      const_cast<double*>(M.data()), &lda, 
      s.data(), u.data(), &rows, vt.data(), &cols, testWork, &lwork, &iwork[0], &info);
  if (info); // handle error conditions here

  // Optimal workspace size is returned in work[0].
  lwork = workSize;
  std::vector<double> work(lwork);

  // Call dgesdd_ to do the actual computation:
  dgesdd_(&workType, &rows, &cols, const_cast<double*>(M.data()), &lda, s.data(), 
    u.data(), &rows, vt.data(), &cols, &work[0], &lwork, &iwork[0], &info);
  if (info); // handle error conditions here

}


/*! @brief Solve linear system of equations
 *
 */
void linalg::solve(Matrix const& matrix, Matrix& rhs) {
  assert(!matrix.permutation_.empty());

  char order = 'N';
  int info = 0;

  int Acols = matrix.cols_, Brows = rhs.rows_, Bcols = rhs.cols_;
  dgetrs_(&order, &Acols, &Brows, 
          const_cast<double*>(&matrix.data_[0]), 
          &Acols, 
          const_cast<int*>(&matrix.permutation_[0]), 
          &rhs.data_[0], 
          &Bcols, 
          &info);
  if (info); // handle error conditions here
}



